//
// Created by echo on 2019/11/26.
//

#include "gpsTools.h"

Eigen::Vector3d gpsTools::GpsMsg2Eigen(const sensor_msgs::NavSatFix &gps_msgs) {
	Eigen::Vector3d
			lla(gps_msgs.latitude, gps_msgs.longitude, gps_msgs.altitude);
	return lla;
}

Eigen::Vector3d gpsTools::LLA2ECEF(const Eigen::Vector3d &lla) {
	Eigen::Vector3d ecef;
	double lat = deg2rad(lla.x());
	double lon = deg2rad(lla.y());
	double alt = lla.z();
	double earth_r = pow(EARTH_MAJOR, 2)
					 / sqrt(pow(EARTH_MAJOR * cos(lat), 2) + pow(EARTH_MINOR * sin(lat), 2));
	ecef.x() = (earth_r + alt) * cos(lat) * cos(lon);
	ecef.y() = (earth_r + alt) * cos(lat) * sin(lon);
	ecef.z() = (pow(EARTH_MINOR / EARTH_MAJOR, 2) * earth_r + alt) * sin(lat);
	
	return ecef;
}

Eigen::Vector3d gpsTools::ECEF2LLA(const Eigen::Vector3d &ecef) {
	double e =
			sqrt((pow(EARTH_MAJOR, 2) - pow(EARTH_MINOR, 2)) / pow(EARTH_MAJOR, 2));
	double e_ =
			sqrt((pow(EARTH_MAJOR, 2) - pow(EARTH_MINOR, 2)) / pow(EARTH_MINOR, 2));
	double p = sqrt(pow(ecef.x(), 2) + pow(ecef.y(), 2));
	double theta = atan2(ecef.z() * EARTH_MAJOR, p * EARTH_MINOR);
	
	double lon = atan2(ecef.y(), ecef.x());
	double lat = atan2((ecef.z() + pow(e_, 2) * EARTH_MINOR * pow(sin(theta), 3)),
					   p - pow(e, 2) * EARTH_MAJOR * pow(cos(theta), 3));
	double earth_r = pow(EARTH_MAJOR, 2)
					 / sqrt(pow(EARTH_MAJOR * cos(lat), 2) + pow(EARTH_MINOR * sin(lat), 2));
	double alt = p / cos(lat) - earth_r;
	Eigen::Vector3d lla(rad2deg(lat), rad2deg(lon), alt);
	return lla;
}

Eigen::Vector3d gpsTools::ECEF2ENU(const Eigen::Vector3d &ecef) {
	double lat = deg2rad(lla_origin_.x());
	double lon = deg2rad(lla_origin_.y());
	
	Eigen::Vector3d t = -LLA2ECEF(lla_origin_);
	Eigen::Matrix3d r;
	r << -sin(lon), cos(lon), 0,
			-cos(lon) * sin(lat), -sin(lat) * sin(lon), cos(lat),
			cos(lon) * cos(lat), sin(lon) * cos(lat), sin(lat);
	
	Eigen::Vector3d enu;
	enu = ecef + t;
	enu = r * enu;
	return enu;
}

Eigen::Vector3d gpsTools::ENU2ECEF(const Eigen::Vector3d &enu) {
	double lat = deg2rad(lla_origin_.x());
	double lon = deg2rad(lla_origin_.y());
	
	Eigen::Vector3d t = LLA2ECEF(lla_origin_);
	Eigen::Matrix3d r;
	r << -sin(lon), -cos(lon) * sin(lat), cos(lon) * cos(lat),
			cos(lon), -sin(lon) * sin(lat), sin(lon) * cos(lat),
			0, cos(lat), sin(lat);
	Eigen::Vector3d ecef;
	ecef = r * enu + t;
	return ecef;
}

void gpsTools::updateGPSpose(const sensor_msgs::NavSatFix &gps_msgs) {
	//检查状态4
	if(gps_msgs.status.status == 4 ||gps_msgs.status.status == 5||gps_msgs.status.status == 1||gps_msgs.status.status == 2){
		//第一个的时候设置为起点
		if(lla_origin_ == Eigen::Vector3d::Identity()){
			Eigen::Vector3d lla  = GpsMsg2Eigen(gps_msgs);
			lla_origin_ = lla;
			std::cout<<"GPS origin: "<<lla_origin_<<"\n status: "<<gps_msgs.status.status<<std::endl;
		} else{
			Eigen::Vector3d lla = GpsMsg2Eigen(gps_msgs);
			Eigen::Vector3d ecef = LLA2ECEF(lla);
			Eigen::Vector3d enu = ECEF2ENU(ecef);
			gps_pos_ = enu;
			std::cout<<"GPS lla_origin_: "<<lla_origin_<<"\n curr"<<gps_pos_<<std::endl;
		}
	}
}
